import numpy as np
from RVO import RVO_update, compute_V_des
import time
from robomaster import robot
import pos_nokov as pos
from utils import *
from position import *
from collections import deque
import copy

pt=poses() # pose target for a team
bp=0 # ball possesion 
catcher=0 # 接球人
sb=deque() # stand by
# ce=0 # closest enemy
ready=[1]*7
eid=0 # 离持球者最近的防守队员

# def run(event):
#     global pt
#     p=pos.p
#     pt=copy.deepcopy(p)
    # global pt, bp
    # event.wait()
    # time.sleep(0.2)
    # logging.basicConfig(level=logging.INFO)
    
    # v_max = [1.5 for i in range(4)]
    # ws_model = dict()
    # ws_model['robot_radius'] = 0.25
    # ws_model['circular_obstacles'] = []
    # while not pos.done:
    #     p_cur = pos.p.arr
    #     v_cur=pos.v.get_arr()
    #     logging.debug(f'current position:{p_cur}')
    #     # print(goal)
    #     v_des = compute_V_des(p_cur, pt.arr, v_max)
    #     # print(V_des)
    #     v_best = RVO_update(p_cur, v_des, v_cur, ws_model)
        
    #     logging.debug(f'best vel first:{v_best}')
    #     for i in range(len(v_best)):
    #         if distance(pt.arr[i], p_cur[i])<0.2:
    #             continue
    #         elif np.linalg.norm(v_best[i])<0.2:
    #             v_best[i]=v_best[i]/np.linalg.norm(v_best[i])*0.2

    #     logging.debug(f'best vel second:{v_best}')
    #     time.sleep(0.05)